/***************************************************************************//**
 * @file camera_array_main.cpp
 * @brief Main entry point for the camera array program
 *
 * @mainpage Camera Array Depth Mapping
 *
 * @authors Brian Fehrman and Scott Logan
 *
 * @date April 17 2013
 * 
 * @section course_infromation Course Information
 * 
 * @par Instructor:
 *         Dr. Weiss
 *
 * @par Course:
 *         Image Processing Spring 2013
 *
 * @par Location:
 *         South Dakota School of Mines and Technology
 *
 * @section program_section Program Information
 *
 * @details This main goal of this program is to allow a user to generate
 * to generate depth maps from an array of cameras. The individual depth maps
 * are then combined in to a single depth map that will hopefully contain more
 * accurate information than any individual depth map alone.
 * 
 * Currently the program could potentially handle up to a 3x3 array of cameras or
 * any connected subset of a 3x3 grid (i.e., 1x1, 1x2, 2x2, 2x1, etc). It was originally
 * designed around a 2x2 array. With some slight modifications to the math used
 * for the difference maps the program could be made to handle an NxM array of
 * cameras that would only be limited by bandwidth issues.
 * 
 * The program has 5 main steps:
   @verbatim
   1.) Initialize Camera Settings
        
   2.) Capture Images

   3.) Calibrate Camera

   4.) Match Pixels in Image Pairs

   5.) Generate Depth Maps
   @endverbatim
 * Each of these five steps are elaborated further in their resepective files. Any of these steps
 * can be enabled/disabled through the use of the camera_array_settings.xml file. Settings for each
 * of the 5 steps listed above can also be controled by updating the camera_array_settings.xml file.
 * The settings file contains many comments in an attempt to help the user easily understand each setting.
 * 
 * @section settings_section Required XML Settings
 * 
 * @verbatim
   [basic_settings][run_init_cams] - disable/enable camera settings initialization (int)
   [basic_settings][run_capture] - disable/enable camera capture (int)
   [basic_settings][run_calibration] - disable/enable camera calibration (int)
   [basic_settings][run_matching] - disable/enable iamge matching (int)
   [basic_settings][run_depth] - disable/enable depth map generation (int)
   [cam_settings] - settings for the camera settings initialization (xml node)
   [cap_settings] - settings for the camera capture (xml node)
   [calib_settings] - settings for camera calibration (xml node)
   [match_settings] - settings for matching (xml node)
   [depth_settings] - settings for depth map generation (xml node)
 * @endverbatim
 *
 * @section compile_section Compiling and Usage
 *
 * @par Compiling Instructions:
 *      This program uses OpenCV, OpenMP, and uvcdynctrl. Please make sure your system has
 *      these dependencies installed. OpenMP isn't completely necessary but the program
 *      is going to run substantially slower without it. The program will compile if you don't
 *      have uvcdynctrl but it is going to crash if you try to run the Initialize Camera Settings step.
 * 
 *      To compile, from within the top level program directory (contains the folders src, include, xml, etc.)
   @verbatim
   make clean
   make all
   @endverbatim
 *       Make sure the Makefile points to your include and lib directories which contain opencv.
 * @par Usage:
 * 
   @verbatim
   ./bin/camera_array [settings_file_name]
   @endverbatim
 *
 * @section todo_bugs_modification_section Todo, Bugs, and Modifications
 *
 * @todo Add feature based mathching
 * @todo Add Qt Front End
 * @todo Add GPU Acceleration
 ******************************************************************************/

///////// Includes ////////////
#include "camera_array_main.h"
void undistort_test();

int main( int argc, char *argv[])
{  
   string settings_file = "xml/camera_array_settings.xml";
   
   if( argc > 1 )
      settings_file = argv[ 1 ];
   
   FileStorage fs( settings_file, FileStorage::READ);
   
   if( ! fs.isOpened() )
   {
      cout << "Error opening settings file: " << settings_file << endl;
      return -1;
   }
   
   int run_init_cams;
   int run_capture;
   int run_calibration;
   int run_matching;
   int run_depth;
   
   fs["basic_settings"]["run_init_cams"] >> run_init_cams;
   fs["basic_settings"]["run_capture"] >> run_capture;
   fs["basic_settings"]["run_calibration"] >> run_calibration;
   fs["basic_settings"]["run_matching"] >> run_matching;
   fs["basic_settings"]["run_depth"] >> run_depth;
   
   if( run_init_cams )
   {
      cout << "**********************************" << endl;
      cout << "INITIALIZING CAMERA SETTINGS" << endl << endl;
      init_cams( fs["cam_settings"] );
      cout << endl << "DONE INITIALIZING CAMERA SETTINGS" << endl;
      cout << "**********************************" << endl;
   }
   
   if( run_capture )
   {
      cout << "**********************************" << endl;
      cout << "RUNNING IMAGE CAPTURE" << endl << endl;
      capture_images( fs["cap_settings"] );
      cout << endl << "DONE RUNNING IMAGE CAPTURE" << endl;
      cout << "**********************************" << endl;
   }
   
   if( run_calibration )
   {
      cout << "**********************************" << endl;
      cout << "RUNNING CAMERA CALIBRATION" << endl << endl;
      calibrate( fs["calib_settings"] );
      cout << endl << "DONE RUNNING CAMERA CALIBRATION" << endl;
      cout << "**********************************" << endl;
   }
   
   if( run_matching )
   {
      cout << "**********************************" << endl;
      cout << "RUNNING MATCHING" << endl << endl;
      camera_array_match( fs[ "match_settings" ] );
      cout << endl << "DONE RUNNING MATCHING" << endl;
      cout << "**********************************" << endl;
   }
   
   if( run_depth )
   {
      cout << "**********************************" << endl;
      cout << "RUNNING DEPTH COMPUTATION" << endl << endl;
      compute_array_depth( fs[ "depth_settings" ] );
      cout << endl << "DONE RUNNING DEPTH COMPUTATION" << endl;
      cout << "**********************************" << endl;
   }
   
   fs.release();
   
   return 0;
}

void undistort_test()
{
   VideoCapture cam( 6 );
   int width = 320;
   int height = 240;
   Mat camera_mat;
   Mat dist_coeffs;
   
   FileStorage fs;
   fs.open( "cam_1_1.xml", FileStorage::READ );
   
   fs[ "camera_mat" ] >> camera_mat;
   fs[ "dist_coeffs" ] >> dist_coeffs;

   cam.set( CV_CAP_PROP_FRAME_WIDTH, width);
   cam.set( CV_CAP_PROP_FRAME_HEIGHT, height );

   namedWindow( "original", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );
   namedWindow( "undistorted", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );

   //Capture loop. Goes until user hits a key.
   for( ;; )
   {
      Mat cap_color;
      Mat undistorted;

      cam >> cap_color;
      undistort( cap_color, undistorted, camera_mat, dist_coeffs );
            
      imshow( "original", cap_color );
      imshow( "undistorted", undistorted );
      
      imwrite( "calib_results/original.jpeg", cap_color );
      imwrite( "calib_results/undistorted.jpeg", undistorted );
      
      //Waits 30ms and exits loop if user hits key
      if(waitKey(30) >= 0) break;
      
   }

}



